#pragma ident "$Id: XY. CAO 2017-08-15 $"

/**
* @file CorrectBiasLC.hpp
* Class to correct observables with satellite upds.
*/

//============================================================================
//
//  shjzhang, 2020/03/01, create for lc-bias correct
//
//============================================================================


#include "CorrectBiasLC.hpp"
#include "constants.hpp"

#define debug 0

namespace gpstk
{
	// Index initially assigned to this class
	int CorrectBiasLC::classIndex = 4900000;


	// Returns an index identifying this object.
	int CorrectBiasLC::getIndex() const
	{ return index; }


	// Returns a string identifying this object.
	std::string CorrectBiasLC::getClassName() const
	{ return "CorrectBiasLC"; }


	/* Returns a satTypeValueMap object, adding the new data generated
	*  when calling this object.
	*
	* @param time      Epoch corresponding to the data.
	* @param gData     Data object holding the data.
	*/
	satTypeValueMap& CorrectBiasLC::Process( const CommonTime& time,
		                                     satTypeValueMap& gData )
        noexcept(false)
	{
		try
		{
			SatIDSet satRejectedSet;

			// Loop through all the satellites
			for (satTypeValueMap::iterator stv = gData.begin(); 
				 stv != gData.end(); 
				 ++stv)
			{
                TypeIDSet typeSetRejected;

                typeValueMap wlBiasData;
                typeValueMap lcBiasData;
				try
				{
					lcBiasData = (*pBiasStore).getValue( (*stv).first, time );

                    if(debug)
                    {
                        for(auto bd: lcBiasData)
                        {
                            cout << " sat: "<< (*stv).first 
                                 << " epoch: " << time 
                                 << " var:" << bd.first
                                 << " bias:" << bd.second
                                 << endl;
                        }
                    }
				}
				catch(InvalidRequest& e)
				{
					// If some problem appears, then schedule this satellite
					// for removal
					satRejectedSet.insert( (*stv).first );
					continue;    // Skip this SV if problems arise
				}

				// Warning:
				// you should check the unit of the UPDs in the input file.
				// if it is cycles, then convert it to meters firstly, 
				// or if it is meters, you can use it directly.

				SatID tempSat = (*stv).first;			
				if( tempSat.system == SatelliteSystem::GPS )
				{
                    try
                    {
                        // unit is cycle
                        double updBLC12 = lcBiasData.getValue(TypeID::BLC12);

                        double LC12 = (*stv).second(TypeID::LC12);

                        // store the bias into data 
				        (*stv).second[TypeID::LC12]  = LC12 - LC12_WAVELENGTH_GPS*updBLC12;
/*
                        double f1, f2;
                        f1 = L1_FREQ_GPS;
                        f2 = L2_FREQ_GPS;

					    // Get updSatL1/updSatL2, unit:cycles
					    double updBL1 = updBLC12 - f2/(f1-f2)*updBWL12;
					    double updBL2 = updBLC12 - f1/(f1-f2)*updBWL12;

                        double L1 = (*it).second(TypeID::L1);
                        double L2 = (*it).second(TypeID::L2);

					    (*stv).second[TypeID::L1] -= L1 - L1_WAVELENGTH_GPS*updBL1;
					    (*stv).second[TypeID::L2] -= L2 - L2_WAVELENGTH_GPS*updBL2;
*/

                    }
                    catch(...)
                    {
                        // If some value is missing, then schedule this satellite
                        // for removal
                        satRejectedSet.insert( tempSat );
                        continue;
                    }

                    try
                    {

                        double updBLC15 = lcBiasData.getValue(TypeID::BLC15);

                        double LC15 = (*stv).second(TypeID::LC15);
                        
				        // Now, correct the Melboune-Wubbena combination observables
				        // for every GNSS satellites.
				        (*stv).second[TypeID::biasSatLC15]= updBLC15;
				        (*stv).second[TypeID::LC15] = LC15 - LC15_WAVELENGTH_GPS*updBLC15;

/*
                        double f1, f5;
                        f1 = L1_FREQ_GPS;
                        f5 = L5_FREQ_GPS;

					    // Get updSatL1/updSatL2, unit:cycles
					    double updBL5 = updBLC15 - f1/(f1-f5)*updBWL15;
                        double L5 = (*it).second(TypeID::L5);

                        // L5 upd
                        (*stv).second[TypeID::L5] -= L5 - L5_WAVELENGTH_GPS*updBL5;
*/

                    }
                    catch(...)
                    {
                        // If some value is missing, then schedule this satellite
                        // for removal
                        typeSetRejected.insert(TypeID::MW15);
                        typeSetRejected.insert(TypeID::LC15);
                        typeSetRejected.insert(TypeID::L5);
                        continue;
                    }

				}
				else if( tempSat.system == SatelliteSystem::Galileo )
				{
				}
				else if( tempSat.system == SatelliteSystem::BDS)
				{
				}	
				else if( tempSat.system == SatelliteSystem::GLONASS)
				{
/*
					int k(0);
					try
					{
						// Get frequency number 
						k = (*stv).second[TypeID::FreqNo];

					}
					catch(InvalidRequest& e)
					{
						// If some problem appears, then schedule this satellite
						// for removal. Thiis may be caused GLONASS not processed
						satRejectedSet.insert( (*stv).first );

						continue;    // Skip this SV if problems arise

					}

					// Frequency for this satellite
					double GLO_L1_FRE = L1_FREQ_GLO + L1_FREQ_STEP_GLO*k;
					double GLO_L2_FRE = L2_FREQ_GLO + L2_FREQ_STEP_GLO*k;
					double L1_WAVE_GLO = C_MPS/GLO_L1_FRE;
					double L2_WAVE_GLO = C_MPS/GLO_L2_FRE;
					double WL_WAVE_GLO = C_MPS/(GLO_L1_FRE - GLO_L2_FRE);
					double NL_WAVE_GLO = C_MPS/(GLO_L1_FRE + GLO_L2_FRE);

					// Change unit from meter to cycles
					updSatWL = satUPD.updSatMW/WL_WAVE_GLO;
					updSatLC = satUPD.updSatLC/NL_WAVE_GLO;

					// Get updSatL1/updSatL2, unit:cycles
					updSatL1 = updSatLC - GLO_L2_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;
					updSatL2 = updSatLC - GLO_L1_FRE/(GLO_L1_FRE-GLO_L2_FRE)*updSatWL;

					// Convert from cycles to meters
					updSatL1 = updSatL1 * L1_WAVE_GLO;
					updSatL2 = updSatL2 * L2_WAVE_GLO;

					(*stv).second[TypeID::updSatL1] = updSatL1;
					(*stv).second[TypeID::updSatL2] = updSatL2;
*/
				}	


			}  // End of 'for (stv = gData.begin(); stv != gData.end(); ++stv)'

			// Remove satellites with missing data
			gData.removeSatID(satRejectedSet);

			// Return
			return gData;

		}
		catch(Exception& u)
		{

			// Throw an exception if something unexpected happens
			ProcessingException e( getClassName() + ":"
				+ StringUtils::asString( getIndex() ) + ":"
				+ u.what() );

			GPSTK_THROW(e);

		}

	}  // End of method 'CorrectBiasLC::Process()'

}

